#include "WPILib.h"
#include "Robot2489.h"

	/**
	 * Let's assign someone to write code for this! (i.e. kick and then move 
	 * out of the way).
	 */

void Robot2489::Autonomous(int zone)
	{
		GetWatchdog().SetEnabled(false);
		KarlKompressor->Start();
		
		// Encoder: 84.7 clicks per inch of forward movement of robot
		for(int x = 0; x < zone; x++)
		{
			MyRobot->SetLeftRightMotorSpeeds(0.5,0.5);
			leftEncoder->Start();
			rightEncoder->Start();
//			bool readyToStop = false;
			int leftEncoderVal;
			int rightEncoderVal;
			while(1)
			{
				leftEncoderVal = leftEncoder->Get();
				rightEncoderVal = rightEncoder->Get();
				if(leftEncoderVal >= 3049 && rightEncoderVal >= 3049) // 36 inches
				{
					leftEncoder->Stop();
					rightEncoder->Stop();
					leftEncoder->Reset();
					rightEncoder->Reset();
					break;
				}
			}
			MyRobot->SetLeftRightMotorSpeeds(0.0,0.0);
			this.kickFunction();
		}
}
